/*!
 * \file CFreeFormDefBox.cpp
 * \brief Subroutines for handling Free-Form Deformation Boxes
 * \author F. Palacios, T. Economon, S. Padron
 * \version 7.1.1 "Blackbird"
 *
 * SU2 Project Website: https://su2code.github.io
 *
 * The SU2 Project is maintained by the SU2 Foundation
 * (http://su2foundation.org)
 *
 * Copyright 2012-2020, SU2 Contributors (cf. AUTHORS.md)
 *
 * SU2 is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * SU2 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with SU2. If not, see <http://www.gnu.org/licenses/>.
 */

#include "../../include/grid_movement/CFreeFormDefBox.hpp"
#include "../../include/grid_movement/CBezierBlending.hpp"
#include "../../include/grid_movement/CBSplineBlending.hpp"

CFreeFormDefBox::CFreeFormDefBox(void) : CGridMovement() { }

CFreeFormDefBox::CFreeFormDefBox(const unsigned short Degree[], unsigned short BSplineOrder[], unsigned short kind_blending) : CGridMovement() {

  unsigned short iCornerPoints, iOrder, jOrder, kOrder, iDim;

  /*--- FFD is always 3D (even in 2D problems) ---*/

  nDim = 3;
  nCornerPoints = 8;

  /*--- Allocate Corners points ---*/

  Coord_Corner_Points = new su2double* [nCornerPoints];
  for (iCornerPoints = 0; iCornerPoints < nCornerPoints; iCornerPoints++)
    Coord_Corner_Points[iCornerPoints] = new su2double [nDim];

  ParamCoord = new su2double[nDim]; ParamCoord_ = new su2double[nDim];
  cart_coord = new su2double[nDim]; cart_coord_ = new su2double[nDim];
  Gradient = new su2double[nDim];

  lDegree = Degree[0]; lOrder = lDegree+1;
  mDegree = Degree[1]; mOrder = mDegree+1;
  nDegree = Degree[2]; nOrder = nDegree+1;
  nControlPoints = lOrder*mOrder*nOrder;

  lDegree_Copy = Degree[0]; lOrder_Copy = lDegree+1;
  mDegree_Copy = Degree[1]; mOrder_Copy = mDegree+1;
  nDegree_Copy = Degree[2]; nOrder_Copy = nDegree+1;
  nControlPoints_Copy = lOrder_Copy*mOrder_Copy*nOrder_Copy;

  Coord_Control_Points = new su2double*** [lOrder];
  ParCoord_Control_Points = new su2double*** [lOrder];
  Coord_Control_Points_Copy = new su2double*** [lOrder];
  for (iOrder = 0; iOrder < lOrder; iOrder++) {
    Coord_Control_Points[iOrder] = new su2double** [mOrder];
    ParCoord_Control_Points[iOrder] = new su2double** [mOrder];
    Coord_Control_Points_Copy[iOrder] = new su2double** [mOrder];
    for (jOrder = 0; jOrder < mOrder; jOrder++) {
      Coord_Control_Points[iOrder][jOrder] = new su2double* [nOrder];
      ParCoord_Control_Points[iOrder][jOrder] = new su2double* [nOrder];
      Coord_Control_Points_Copy[iOrder][jOrder] = new su2double* [nOrder];
      for (kOrder = 0; kOrder < nOrder; kOrder++) {
        Coord_Control_Points[iOrder][jOrder][kOrder] = new su2double [nDim];
        ParCoord_Control_Points[iOrder][jOrder][kOrder] = new su2double [nDim];
        Coord_Control_Points_Copy[iOrder][jOrder][kOrder] = new su2double [nDim];
        for (iDim = 0; iDim < nDim; iDim++) {
          Coord_Control_Points[iOrder][jOrder][kOrder][iDim] = 0.0;
          ParCoord_Control_Points[iOrder][jOrder][kOrder][iDim] = 0.0;
          Coord_Control_Points_Copy[iOrder][jOrder][kOrder][iDim] = 0.0;
        }
      }
    }
  }

  BlendingFunction = new CFreeFormBlending*[nDim];

  if (kind_blending == BEZIER){
    BlendingFunction[0] = new CBezierBlending(lOrder, lOrder);
    BlendingFunction[1] = new CBezierBlending(mOrder, mOrder);
    BlendingFunction[2] = new CBezierBlending(nOrder, nOrder);
  }
  if (kind_blending == BSPLINE_UNIFORM){
    BlendingFunction[0] = new CBSplineBlending(BSplineOrder[0], lOrder);
    BlendingFunction[1] = new CBSplineBlending(BSplineOrder[1], mOrder);
    BlendingFunction[2] = new CBSplineBlending(BSplineOrder[2], nOrder);
  }

}

CFreeFormDefBox::~CFreeFormDefBox(void) {
  unsigned short iOrder, jOrder, kOrder, iCornerPoints, iDim;

  for (iOrder = 0; iOrder < lOrder; iOrder++)
    for (jOrder = 0; jOrder < mOrder; jOrder++)
      for (kOrder = 0; kOrder < nOrder; kOrder++) {
        delete [] Coord_Control_Points[iOrder][jOrder][kOrder];
        delete [] ParCoord_Control_Points[iOrder][jOrder][kOrder];
        delete [] Coord_Control_Points_Copy[iOrder][jOrder][kOrder];
      }
  delete [] Coord_Control_Points;
  delete [] ParCoord_Control_Points;
  delete [] Coord_Control_Points_Copy;

  delete [] ParamCoord;
  delete [] cart_coord;
  delete [] Gradient;

  for (iCornerPoints = 0; iCornerPoints < nCornerPoints; iCornerPoints++)
    delete [] Coord_Corner_Points[iCornerPoints];
  delete [] Coord_Corner_Points;

  for (iDim = 0; iDim < nDim; iDim++){
    delete BlendingFunction[iDim];
  }
  delete [] BlendingFunction;
}

void  CFreeFormDefBox::SetUnitCornerPoints(void) {

  unsigned short iDim;
  su2double *coord = new su2double [nDim];

  for (iDim = 0; iDim < nDim; iDim++) coord[iDim] = 0.0;

  coord [0] = 0.0; coord [1] = 0.0; coord [2] = 0.0; this->SetCoordCornerPoints(coord, 0);
  coord [0] = 1.0; coord [1] = 0.0; coord [2] = 0.0; this->SetCoordCornerPoints(coord, 1);
  coord [0] = 1.0; coord [1] = 1.0; coord [2] = 0.0; this->SetCoordCornerPoints(coord, 2);
  coord [0] = 0.0; coord [1] = 1.0; coord [2] = 0.0; this->SetCoordCornerPoints(coord, 3);
  coord [0] = 0.0; coord [1] = 0.0; coord [2] = 1.0; this->SetCoordCornerPoints(coord, 4);
  coord [0] = 1.0; coord [1] = 0.0; coord [2] = 1.0; this->SetCoordCornerPoints(coord, 5);
  coord [0] = 1.0; coord [1] = 1.0; coord [2] = 1.0; this->SetCoordCornerPoints(coord, 6);
  coord [0] = 0.0; coord [1] = 1.0; coord [2] = 1.0; this->SetCoordCornerPoints(coord, 7);

  delete [] coord;

}

void CFreeFormDefBox::SetControlPoints_Parallelepiped (void) {
  unsigned short iDim, iDegree, jDegree, kDegree;

  /*--- Set base control points according to the notation of Vtk for hexahedrons ---*/
  for (iDim = 0; iDim < nDim; iDim++) {
    Coord_Control_Points  [0]      [0]      [0]      [iDim]  = Coord_Corner_Points[0][iDim];
    Coord_Control_Points  [lOrder-1]  [0]      [0]      [iDim]  = Coord_Corner_Points[1][iDim];
    Coord_Control_Points  [lOrder-1]  [mOrder-1]  [0]      [iDim]  = Coord_Corner_Points[2][iDim];
    Coord_Control_Points  [0]      [mOrder-1]  [0]      [iDim]  = Coord_Corner_Points[3][iDim];
    Coord_Control_Points  [0]      [0]      [nOrder-1]  [iDim]  = Coord_Corner_Points[4][iDim];
    Coord_Control_Points  [lOrder-1]  [0]      [nOrder-1]  [iDim]  = Coord_Corner_Points[5][iDim];
    Coord_Control_Points  [lOrder-1]  [mOrder-1]  [nOrder-1]  [iDim]  = Coord_Corner_Points[6][iDim];
    Coord_Control_Points  [0]      [mOrder-1]  [nOrder-1]  [iDim]  = Coord_Corner_Points[7][iDim];
  }

  /*--- Fill the rest of the cubic matrix of control points with uniform spacing (parallelepiped) ---*/
  for (iDegree = 0; iDegree <= lDegree; iDegree++)
    for (jDegree = 0; jDegree <= mDegree; jDegree++)
      for (kDegree = 0; kDegree <= nDegree; kDegree++) {
        Coord_Control_Points[iDegree][jDegree][kDegree][0] = Coord_Corner_Points[0][0]
        + su2double(iDegree)/su2double(lDegree)*(Coord_Corner_Points[1][0]-Coord_Corner_Points[0][0]);
        Coord_Control_Points[iDegree][jDegree][kDegree][1] = Coord_Corner_Points[0][1]
        + su2double(jDegree)/su2double(mDegree)*(Coord_Corner_Points[3][1]-Coord_Corner_Points[0][1]);
        Coord_Control_Points[iDegree][jDegree][kDegree][2] = Coord_Corner_Points[0][2]
        + su2double(kDegree)/su2double(nDegree)*(Coord_Corner_Points[4][2]-Coord_Corner_Points[0][2]);
      }
}

void CFreeFormDefBox::SetSupportCP(CFreeFormDefBox *FFDBox) {
  unsigned short iDim, iOrder, jOrder, kOrder;
  unsigned short lOrder = FFDBox->GetlOrder();
  unsigned short mOrder = FFDBox->GetmOrder();
  unsigned short nOrder = FFDBox->GetnOrder();

  Coord_SupportCP = new su2double*** [lOrder];
  for (iOrder = 0; iOrder < lOrder; iOrder++) {
    Coord_SupportCP[iOrder] = new su2double** [mOrder];
    for (jOrder = 0; jOrder < mOrder; jOrder++) {
      Coord_SupportCP[iOrder][jOrder] = new su2double* [nOrder];
      for (kOrder = 0; kOrder < nOrder; kOrder++)
        Coord_SupportCP[iOrder][jOrder][kOrder] = new su2double [nDim];
    }
  }

  /*--- Set base support control points according to the notation of Vtk for hexahedrons ---*/
  for (iDim = 0; iDim < nDim; iDim++) {
    Coord_SupportCP  [0]      [0]      [0]      [iDim]  = Coord_Corner_Points[0][iDim];
    Coord_SupportCP  [lOrder-1]  [0]      [0]      [iDim]  = Coord_Corner_Points[1][iDim];
    Coord_SupportCP  [lOrder-1]  [mOrder-1]  [0]      [iDim]  = Coord_Corner_Points[2][iDim];
    Coord_SupportCP  [0]      [mOrder-1]  [0]      [iDim]  = Coord_Corner_Points[3][iDim];
    Coord_SupportCP  [0]      [0]      [nOrder-1]  [iDim]  = Coord_Corner_Points[4][iDim];
    Coord_SupportCP  [lOrder-1]  [0]      [nOrder-1]  [iDim]  = Coord_Corner_Points[5][iDim];
    Coord_SupportCP  [lOrder-1]  [mOrder-1]  [nOrder-1]  [iDim]  = Coord_Corner_Points[6][iDim];
    Coord_SupportCP  [0]      [mOrder-1]  [nOrder-1]  [iDim]  = Coord_Corner_Points[7][iDim];
  }

  /*--- Fill the rest of the cubic matrix of support control points with uniform spacing  ---*/
  for (iOrder = 0; iOrder < lOrder; iOrder++)
    for (jOrder = 0; jOrder < mOrder; jOrder++)
      for (kOrder = 0; kOrder < nOrder; kOrder++) {
        Coord_SupportCP[iOrder][jOrder][kOrder][0] = Coord_Corner_Points[0][0]
        + su2double(iOrder)/su2double(lOrder-1)*(Coord_Corner_Points[1][0]-Coord_Corner_Points[0][0]);
        Coord_SupportCP[iOrder][jOrder][kOrder][1] = Coord_Corner_Points[0][1]
        + su2double(jOrder)/su2double(mOrder-1)*(Coord_Corner_Points[3][1]-Coord_Corner_Points[0][1]);
        Coord_SupportCP[iOrder][jOrder][kOrder][2] = Coord_Corner_Points[0][2]
        + su2double(kOrder)/su2double(nOrder-1)*(Coord_Corner_Points[4][2]-Coord_Corner_Points[0][2]);
      }
}

void CFreeFormDefBox::SetSupportCPChange(CFreeFormDefBox *FFDBox) {
  unsigned short iDim, iOrder, jOrder, kOrder;
  su2double *CartCoordNew, *ParamCoord;
  unsigned short lOrder = FFDBox->GetlOrder();
  unsigned short mOrder = FFDBox->GetmOrder();
  unsigned short nOrder = FFDBox->GetnOrder();

  su2double ****ParamCoord_SupportCP = new su2double*** [lOrder];
  for (iOrder = 0; iOrder < lOrder; iOrder++) {
    ParamCoord_SupportCP[iOrder] = new su2double** [mOrder];
    for (jOrder = 0; jOrder < mOrder; jOrder++) {
      ParamCoord_SupportCP[iOrder][jOrder] = new su2double* [nOrder];
      for (kOrder = 0; kOrder < nOrder; kOrder++)
        ParamCoord_SupportCP[iOrder][jOrder][kOrder] = new su2double [nDim];
    }
  }

  for (iOrder = 0; iOrder < lOrder; iOrder++)
    for (jOrder = 0; jOrder < mOrder; jOrder++)
      for (kOrder = 0; kOrder < nOrder; kOrder++)
        for (iDim = 0; iDim < nDim; iDim++)
          ParamCoord_SupportCP[iOrder][jOrder][kOrder][iDim] =
          Coord_SupportCP[iOrder][jOrder][kOrder][iDim];

  for (iDim = 0; iDim < nDim; iDim++) {
    Coord_Control_Points[0][0][0][iDim]  = FFDBox->GetCoordCornerPoints(iDim, 0);
    Coord_Control_Points[1][0][0][iDim]  = FFDBox->GetCoordCornerPoints(iDim, 1);
    Coord_Control_Points[1][1][0][iDim]  = FFDBox->GetCoordCornerPoints(iDim, 2);
    Coord_Control_Points[0][1][0][iDim]  = FFDBox->GetCoordCornerPoints(iDim, 3);
    Coord_Control_Points[0][0][1][iDim]  = FFDBox->GetCoordCornerPoints(iDim, 4);
    Coord_Control_Points[1][0][1][iDim]  = FFDBox->GetCoordCornerPoints(iDim, 5);
    Coord_Control_Points[1][1][1][iDim]  = FFDBox->GetCoordCornerPoints(iDim, 6);
    Coord_Control_Points[0][1][1][iDim]  = FFDBox->GetCoordCornerPoints(iDim, 7);
  }

  for (iOrder = 0; iOrder < FFDBox->GetlOrder(); iOrder++) {
    for (jOrder = 0; jOrder < FFDBox->GetmOrder(); jOrder++) {
      for (kOrder = 0; kOrder < FFDBox->GetnOrder(); kOrder++) {
        ParamCoord = ParamCoord_SupportCP[iOrder][jOrder][kOrder];
        CartCoordNew = EvalCartesianCoord(ParamCoord);
        FFDBox->SetCoordControlPoints(CartCoordNew, iOrder, jOrder, kOrder);
        FFDBox->SetCoordControlPoints_Copy(CartCoordNew, iOrder, jOrder, kOrder);
      }
    }
  }

}

void CFreeFormDefBox::SetCart2Cyl_ControlPoints(CConfig *config) {

  unsigned short iDegree, jDegree, kDegree;
  su2double CartCoord[3];
  su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;

  X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

  for (kDegree = 0; kDegree <= nDegree; kDegree++) {
    for (jDegree = 0; jDegree <= mDegree; jDegree++) {
      for (iDegree = 0; iDegree <= lDegree; iDegree++) {

        CartCoord[0] = Coord_Control_Points[iDegree][jDegree][kDegree][0];
        CartCoord[1] = Coord_Control_Points[iDegree][jDegree][kDegree][1];
        CartCoord[2] = Coord_Control_Points[iDegree][jDegree][kDegree][2];

        Xbar =  CartCoord[0] - X_0; Ybar =  CartCoord[1] - Y_0; Zbar =  CartCoord[2] - Z_0;

        Coord_Control_Points[iDegree][jDegree][kDegree][0] = sqrt(Ybar*Ybar + Zbar*Zbar);
        Coord_Control_Points[iDegree][jDegree][kDegree][1] = atan2 ( Zbar, Ybar);
        if (Coord_Control_Points[iDegree][jDegree][kDegree][1] > PI_NUMBER/2.0) Coord_Control_Points[iDegree][jDegree][kDegree][1] -= 2.0*PI_NUMBER;
        Coord_Control_Points[iDegree][jDegree][kDegree][2] = Xbar;

        CartCoord[0] = Coord_Control_Points_Copy[iDegree][jDegree][kDegree][0];
        CartCoord[1] = Coord_Control_Points_Copy[iDegree][jDegree][kDegree][1];
        CartCoord[2] = Coord_Control_Points_Copy[iDegree][jDegree][kDegree][2];

        Xbar =  CartCoord[0] - X_0; Ybar =  CartCoord[1] - Y_0; Zbar =  CartCoord[2] - Z_0;

        Coord_Control_Points_Copy[iDegree][jDegree][kDegree][0] = sqrt(Ybar*Ybar + Zbar*Zbar);
        Coord_Control_Points_Copy[iDegree][jDegree][kDegree][1] = atan2 (Zbar, Ybar);
        if (Coord_Control_Points_Copy[iDegree][jDegree][kDegree][1] > PI_NUMBER/2.0) Coord_Control_Points_Copy[iDegree][jDegree][kDegree][1] -= 2.0*PI_NUMBER;
        Coord_Control_Points_Copy[iDegree][jDegree][kDegree][2] = Xbar;

      }
    }
  }

}

void CFreeFormDefBox::SetCyl2Cart_ControlPoints(CConfig *config) {

  unsigned short iDegree, jDegree, kDegree;
  su2double PolarCoord[3];

   su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;
  X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

  for (kDegree = 0; kDegree <= nDegree; kDegree++) {
    for (jDegree = 0; jDegree <= mDegree; jDegree++) {
      for (iDegree = 0; iDegree <= lDegree; iDegree++) {


         PolarCoord[0] = Coord_Control_Points[iDegree][jDegree][kDegree][0];
         PolarCoord[1] = Coord_Control_Points[iDegree][jDegree][kDegree][1];
         PolarCoord[2] = Coord_Control_Points[iDegree][jDegree][kDegree][2];


        Xbar = PolarCoord[2];
        Ybar = PolarCoord[0] * cos(PolarCoord[1]);
        Zbar = PolarCoord[0] * sin(PolarCoord[1]);

        PolarCoord[0] =  Xbar +X_0;  PolarCoord[1] = Ybar +Y_0; PolarCoord[2] = Zbar +Z_0;

        Coord_Control_Points[iDegree][jDegree][kDegree][0] = PolarCoord[0];
        Coord_Control_Points[iDegree][jDegree][kDegree][1] = PolarCoord[1];
        Coord_Control_Points[iDegree][jDegree][kDegree][2] = PolarCoord[2];

      }
    }
  }

}

void CFreeFormDefBox::SetCart2Cyl_CornerPoints(CConfig *config) {

  unsigned short iCornerPoint;
  su2double *CartCoord;
  su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;

  X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

  for (iCornerPoint = 0; iCornerPoint < 8; iCornerPoint++) {

    CartCoord = GetCoordCornerPoints(iCornerPoint);
    Xbar =  CartCoord[0] - X_0; Ybar =  CartCoord[1] - Y_0; Zbar =  CartCoord[2] - Z_0;

    CartCoord[0] = sqrt(Ybar*Ybar + Zbar*Zbar);
    CartCoord[1] = atan2 ( Zbar, Ybar); if (CartCoord[1] > PI_NUMBER/2.0) CartCoord[1] -= 2.0*PI_NUMBER;
    CartCoord[2] =  Xbar;

  }

}


void CFreeFormDefBox::SetCyl2Cart_CornerPoints(CConfig *config) {

  unsigned short iCornerPoint;
  su2double *PolarCoord;
  su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;

  X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

  for (iCornerPoint = 0; iCornerPoint < 8; iCornerPoint++) {

    PolarCoord = GetCoordCornerPoints(iCornerPoint);

    Xbar = PolarCoord[2];
    Ybar = PolarCoord[0] * cos(PolarCoord[1]);
    Zbar = PolarCoord[0] * sin(PolarCoord[1]);

    PolarCoord[0] =  Xbar + X_0;  PolarCoord[1] = Ybar + Y_0; PolarCoord[2] = Zbar + Z_0;

  }

}

void CFreeFormDefBox::SetCart2Sphe_ControlPoints(CConfig *config) {

  unsigned short iDegree, jDegree, kDegree;
  su2double CartCoord[3];
  su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;

  X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

  for (kDegree = 0; kDegree <= nDegree; kDegree++) {
    for (jDegree = 0; jDegree <= mDegree; jDegree++) {
      for (iDegree = 0; iDegree <= lDegree; iDegree++) {

        CartCoord[0] = Coord_Control_Points[iDegree][jDegree][kDegree][0];
        CartCoord[1] = Coord_Control_Points[iDegree][jDegree][kDegree][1];
        CartCoord[2] = Coord_Control_Points[iDegree][jDegree][kDegree][2];

        Xbar =  CartCoord[0] - X_0; Ybar =  CartCoord[1] - Y_0; Zbar =  CartCoord[2] - Z_0;

        Coord_Control_Points[iDegree][jDegree][kDegree][0] = sqrt(Xbar*Xbar + Ybar*Ybar + Zbar*Zbar);
        Coord_Control_Points[iDegree][jDegree][kDegree][1] = atan2 ( Zbar, Ybar);
        if (Coord_Control_Points[iDegree][jDegree][kDegree][1] > PI_NUMBER/2.0) Coord_Control_Points[iDegree][jDegree][kDegree][1] -= 2.0*PI_NUMBER;
        Coord_Control_Points[iDegree][jDegree][kDegree][2] = acos(Xbar/Coord_Control_Points[iDegree][jDegree][kDegree][0] );

        CartCoord[0] = Coord_Control_Points_Copy[iDegree][jDegree][kDegree][0];
        CartCoord[1] = Coord_Control_Points_Copy[iDegree][jDegree][kDegree][1];
        CartCoord[2] = Coord_Control_Points_Copy[iDegree][jDegree][kDegree][2];

        Xbar =  CartCoord[0] - X_0; Ybar =  CartCoord[1] - Y_0; Zbar =  CartCoord[2] - Z_0;

        Coord_Control_Points_Copy[iDegree][jDegree][kDegree][0] = sqrt(Xbar*Xbar + Ybar*Ybar + Zbar*Zbar);
        Coord_Control_Points_Copy[iDegree][jDegree][kDegree][1] = atan2 ( Zbar, Ybar);
        if (Coord_Control_Points_Copy[iDegree][jDegree][kDegree][1] > PI_NUMBER/2.0)
          Coord_Control_Points_Copy[iDegree][jDegree][kDegree][1]  -= 2.0*PI_NUMBER;
        Coord_Control_Points_Copy[iDegree][jDegree][kDegree][2] = acos(Xbar/Coord_Control_Points_Copy[iDegree][jDegree][kDegree][0]);

      }
    }
  }

}

void CFreeFormDefBox::SetSphe2Cart_ControlPoints(CConfig *config) {

  unsigned short iDegree, jDegree, kDegree;
  su2double PolarCoord[3];

   su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;
  X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

  for (kDegree = 0; kDegree <= nDegree; kDegree++) {
    for (jDegree = 0; jDegree <= mDegree; jDegree++) {
      for (iDegree = 0; iDegree <= lDegree; iDegree++) {

        PolarCoord[0] = Coord_Control_Points[iDegree][jDegree][kDegree][0];
        PolarCoord[1] = Coord_Control_Points[iDegree][jDegree][kDegree][1];
        PolarCoord[2] = Coord_Control_Points[iDegree][jDegree][kDegree][2];

        Xbar = PolarCoord[0] * cos(PolarCoord[2]);
        Ybar = PolarCoord[0] * cos(PolarCoord[1]) * sin(PolarCoord[2]);
        Zbar = PolarCoord[0] * sin(PolarCoord[1]) * sin(PolarCoord[2]);

        PolarCoord[0] = Xbar + X_0;  PolarCoord[1] = Ybar + Y_0; PolarCoord[2] = Zbar + Z_0;

        Coord_Control_Points[iDegree][jDegree][kDegree][0] = PolarCoord[0];
        Coord_Control_Points[iDegree][jDegree][kDegree][1] = PolarCoord[1];
        Coord_Control_Points[iDegree][jDegree][kDegree][2] = PolarCoord[2];

      }
    }
  }

}

void CFreeFormDefBox::SetCart2Sphe_CornerPoints(CConfig *config) {

  unsigned short iCornerPoint;
  su2double *CartCoord;
  su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;

  X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

  for (iCornerPoint = 0; iCornerPoint < 8; iCornerPoint++) {

    CartCoord = GetCoordCornerPoints(iCornerPoint);
    Xbar =  CartCoord[0] - X_0; Ybar =  CartCoord[1] - Y_0; Zbar =  CartCoord[2] - Z_0;

    CartCoord[0] = sqrt(Xbar*Xbar + Ybar*Ybar + Zbar*Zbar);
    CartCoord[1] = atan2(Zbar, Ybar);  if (CartCoord[1] > PI_NUMBER/2.0) CartCoord[1] -= 2.0*PI_NUMBER;
    CartCoord[2] = acos(Xbar/CartCoord[0]);

  }

}


void CFreeFormDefBox::SetSphe2Cart_CornerPoints(CConfig *config) {

  unsigned short iCornerPoint;
  su2double *PolarCoord;
  su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;

  X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

  for (iCornerPoint = 0; iCornerPoint < 8; iCornerPoint++) {

    PolarCoord = GetCoordCornerPoints(iCornerPoint);

    Xbar = PolarCoord[0] * cos(PolarCoord[2]);
    Ybar = PolarCoord[0] * cos(PolarCoord[1]) * sin(PolarCoord[2]);
    Zbar = PolarCoord[0] * sin(PolarCoord[1]) * sin(PolarCoord[2]);

    PolarCoord[0] =  Xbar + X_0;  PolarCoord[1] = Ybar + Y_0; PolarCoord[2] = Zbar + Z_0;

  }

}


void CFreeFormDefBox::SetCGNS(CGeometry *geometry, unsigned short iFFDBox, bool original) {
#ifdef HAVE_CGNS

  char FFDBox_filename[MAX_STRING_SIZE];
  bool new_file;
  unsigned short iDim, iDegree, jDegree, kDegree;
  unsigned short outDim;
  unsigned int pos;
  char zonename[33];
  int FFDBox_cgns_file;
  int cell_dim, phys_dim;
  int cgns_base=0, cgns_family, cgns_zone, cgns_err, dummy;
  const char * basename;

  /*--- FFD output is always 3D (even in 2D problems),
   this is important for debuging ---*/
  nDim = geometry->GetnDim();
  cell_dim = nDim,
  phys_dim = 3;

  SPRINTF (FFDBox_filename, "ffd_boxes.cgns");

  if ((original) && (iFFDBox == 0)) new_file = true;
  else new_file = false;

  if (new_file) {
    cgns_err = cg_open(FFDBox_filename, CG_MODE_WRITE, &FFDBox_cgns_file);
    if (cgns_err) cg_error_print();
    cgns_err = cg_descriptor_write("Title", "Visualization of the FFD boxes generated by SU2_DEF." );
    if (cgns_err) cg_error_print();
  }
  else {
    cgns_err = cg_open(FFDBox_filename, CG_MODE_MODIFY, &FFDBox_cgns_file);
    if (cgns_err) cg_error_print();
  }

  if (original) {
    basename = "Original_FFD";
  }
  else {
    basename = "Deformed_FFD";
  }

  if (iFFDBox == 0){
    cgns_err = cg_base_write(FFDBox_cgns_file, basename, cell_dim, phys_dim, &cgns_base);
    if (cgns_err) cg_error_print();
  }
  cgns_err = cg_family_write(FFDBox_cgns_file, cgns_base, Tag.c_str(), &cgns_family);
  if (cgns_err) cg_error_print();

  cgsize_t dims[9];
  dims[0] = lDegree+1;
  dims[1] = mDegree+1;
  if (cell_dim == 3){
     dims[2] = nDegree+1;
  }
  cgsize_t pointlen = 1;
  for(int ii=0; ii<cell_dim; ii++){
    dims[ii+cell_dim] = dims[ii] -1;
    dims[ii+2*cell_dim] = 0;
    pointlen *= dims[ii];
  }

  passivedouble *buffer = new passivedouble[pointlen];
  SPRINTF (zonename, "SU2_Zone_%d", SU2_TYPE::Int(iFFDBox));

  cgns_err = cg_zone_write(FFDBox_cgns_file, cgns_base, zonename, dims, CGNS_ENUMV(Structured), &cgns_zone);
  if (cgns_err) cg_error_print();
  cgns_err = cg_goto(FFDBox_cgns_file, cgns_base, zonename, 0, nullptr);
  if (cgns_err) cg_error_print();
  cgns_err = cg_famname_write(Tag.c_str());
  if (cgns_err) cg_error_print();


  const char* coord_names[3] = { "CoordinateX", "CoordinateY", "CoordinateZ" };
  for (iDim=0; iDim<nDim; iDim++)
  {
    outDim = nDim == 2 ? 0: nDegree;
    for (kDegree = 0; kDegree <= outDim; kDegree++) {
      for (jDegree = 0; jDegree <= mDegree; jDegree++) {
        for (iDegree = 0; iDegree <= lDegree; iDegree++) {
          pos = iDegree + jDegree*(lDegree+1)+ kDegree*(lDegree+1)*(mDegree+1);
          buffer[pos] = SU2_TYPE::GetValue(Coord_Control_Points[iDegree][jDegree][kDegree][iDim]);
        }
      }
    }
    cgns_err = cg_coord_write(FFDBox_cgns_file, cgns_base, cgns_zone, CGNS_ENUMV(RealDouble), coord_names[iDim], buffer, &dummy);
    if (cgns_err) cg_error_print();

  }
  if (nDim==2){
    std::fill_n(buffer, pointlen, 0.0);
    cgns_err = cg_coord_write(FFDBox_cgns_file, cgns_base, cgns_zone, CGNS_ENUMV(RealDouble), coord_names[2], buffer, &dummy);
    if (cgns_err) cg_error_print();
  }

  delete [] buffer;
  cgns_err = cg_close(FFDBox_cgns_file);
  if (cgns_err) cg_error_print();
#else // Not built with CGNS support
  cout << "CGNS file requested for FFD but SU2 was built without CGNS support. No file written" << "\n";
#endif
}


void CFreeFormDefBox::SetTecplot(CGeometry *geometry, unsigned short iFFDBox, bool original) {

  ofstream FFDBox_file;
  char FFDBox_filename[MAX_STRING_SIZE];
  bool new_file;
  unsigned short iDim, iDegree, jDegree, kDegree;


  /*--- FFD output is always 3D (even in 2D problems),
   this is important for debuging ---*/

  nDim = 3;

  SPRINTF (FFDBox_filename, "ffd_boxes.dat");

  if ((original) && (iFFDBox == 0)) new_file = true;
  else new_file = false;

  if (new_file) {
    FFDBox_file.open(FFDBox_filename, ios::out);
    FFDBox_file << "TITLE = \"Visualization of the FFD boxes generated by SU2_DEF.\"" << endl;
    if (nDim == 2) FFDBox_file << "VARIABLES = \"x\", \"y\"" << endl;
    else FFDBox_file << "VARIABLES = \"x\", \"y\", \"z\"" << endl;
  }
  else FFDBox_file.open(FFDBox_filename, ios::out | ios::app);

  FFDBox_file << "ZONE T= \"" << Tag;
  if (original) FFDBox_file << " (Original FFD)\"";
  else FFDBox_file << " (Deformed FFD)\"";
  if (nDim == 2) FFDBox_file << ", I="<<lDegree+1<<", J="<<mDegree+1<<", DATAPACKING=POINT" << endl;
  else FFDBox_file << ", I="<<lDegree+1<<", J="<<mDegree+1<<", K="<<nDegree+1<<", DATAPACKING=POINT" << endl;

  FFDBox_file.precision(15);

  if (nDim == 2) {
    for (jDegree = 0; jDegree <= mDegree; jDegree++) {
      for (iDegree = 0; iDegree <= lDegree; iDegree++) {
        for (iDim = 0; iDim < nDim; iDim++)
          FFDBox_file << scientific << Coord_Control_Points[iDegree][jDegree][0][iDim] << "\t";
        FFDBox_file << "\n";
      }
    }
  }
  else {
    for (kDegree = 0; kDegree <= nDegree; kDegree++) {
      for (jDegree = 0; jDegree <= mDegree; jDegree++) {
        for (iDegree = 0; iDegree <= lDegree; iDegree++) {
          for (iDim = 0; iDim < nDim; iDim++)
            FFDBox_file << scientific << Coord_Control_Points[iDegree][jDegree][kDegree][iDim] << "\t";
          FFDBox_file << "\n";
        }
      }
    }
  }

  FFDBox_file.close();
}

void CFreeFormDefBox::SetParaview(CGeometry *geometry, unsigned short iFFDBox, bool original) {

  ofstream FFDBox_file;
  char FFDBox_filename[MAX_STRING_SIZE];
  bool new_file;
  unsigned short iDim, iDegree, jDegree, kDegree;

  nDim = geometry->GetnDim();

  if (original) new_file = true;
  else new_file = false;

  if (new_file) SPRINTF (FFDBox_filename, "ffd_boxes_%d.vtk", SU2_TYPE::Int(iFFDBox));
  else SPRINTF (FFDBox_filename, "ffd_boxes_def_%d.vtk", SU2_TYPE::Int(iFFDBox));

  FFDBox_file.open(FFDBox_filename, ios::out);
  FFDBox_file << "# vtk DataFile Version 3.0" << endl;
  FFDBox_file << "vtk output" << endl;
  FFDBox_file << "ASCII" << endl;
  FFDBox_file << "DATASET STRUCTURED_GRID" << endl;

  if (nDim == 2) FFDBox_file << "DIMENSIONS "<<lDegree+1<<" "<<mDegree+1<<" "<<1<< endl;
  else FFDBox_file << "DIMENSIONS "<<lDegree+1<<" "<<mDegree+1<<" "<<nDegree+1<< endl;
  if (nDim == 2) FFDBox_file << "POINTS "<<(lDegree+1)*(mDegree+1)<<" float"<< endl;
  else FFDBox_file << "POINTS "<<(lDegree+1)*(mDegree+1)*(nDegree+1)<<" float"<< endl;

  FFDBox_file.precision(15);

  if (nDim == 2) {
    for (jDegree = 0; jDegree <= mDegree; jDegree++) {
      for (iDegree = 0; iDegree <= lDegree; iDegree++) {
        for (iDim = 0; iDim < nDim; iDim++)
        FFDBox_file << scientific << Coord_Control_Points[iDegree][jDegree][0][iDim] << "\t";
        FFDBox_file << " 0.0 \n";
      }
    }
  }
  else {
    for (kDegree = 0; kDegree <= nDegree; kDegree++) {
      for (jDegree = 0; jDegree <= mDegree; jDegree++) {
        for (iDegree = 0; iDegree <= lDegree; iDegree++) {
          for (iDim = 0; iDim < nDim; iDim++)
          FFDBox_file << scientific << Coord_Control_Points[iDegree][jDegree][kDegree][iDim] << "\t";
          FFDBox_file << "\n";
        }
      }
    }
  }

  FFDBox_file.close();

}

su2double *CFreeFormDefBox::GetParametricCoord_Analytical(const su2double *cart_coord) {
  unsigned short iDim;
  su2double *e1, *e2, *e3, *e12, *e23, *e13, *p;

  /*--- Auxiliary Basis Vectors of the deformed FFDBox ---*/
  e1 = new su2double[3]; e2 = new su2double[3]; e3 = new su2double[3];
  for (iDim = 0; iDim < nDim; iDim++) {
    e1[iDim] = Coord_Corner_Points[1][iDim]-Coord_Corner_Points[0][iDim];
    e2[iDim] = Coord_Corner_Points[3][iDim]-Coord_Corner_Points[0][iDim];
    e3[iDim] = Coord_Corner_Points[4][iDim]-Coord_Corner_Points[0][iDim];
  }

  /*--- Respective Cross-Products ---*/
  e12 = new su2double[3]; e23 = new su2double[3]; e13 = new su2double[3];
  CrossProduct(e1, e2, e12);
  CrossProduct(e1, e3, e13);
  CrossProduct(e2, e3, e23);

  /*--- p is Tranlated vector from the origin ---*/
  p = new su2double[3];
  for (iDim = 0; iDim < nDim; iDim++)
    p[iDim] = cart_coord[iDim] - Coord_Corner_Points[0][iDim];

  ParamCoord[0] = DotProduct(e23, p)/DotProduct(e23, e1);
  ParamCoord[1] = DotProduct(e13, p)/DotProduct(e13, e2);
  ParamCoord[2] = DotProduct(e12, p)/DotProduct(e12, e3);

  delete [] e1;
  delete [] e2;
  delete [] e3;
  delete [] e12;
  delete [] e23;
  delete [] e13;
  delete [] p;

  return ParamCoord;
}

su2double *CFreeFormDefBox::EvalCartesianCoord(su2double *ParamCoord) const {
  unsigned short iDim, iDegree, jDegree, kDegree;

  for (iDim = 0; iDim < nDim; iDim++)
    cart_coord[iDim] = 0.0;

  for (iDegree = 0; iDegree <= lDegree; iDegree++)
    for (jDegree = 0; jDegree <= mDegree; jDegree++)
      for (kDegree = 0; kDegree <= nDegree; kDegree++)
        for (iDim = 0; iDim < nDim; iDim++) {
          cart_coord[iDim] += Coord_Control_Points[iDegree][jDegree][kDegree][iDim]
          * BlendingFunction[0]->GetBasis(iDegree, ParamCoord[0])
          * BlendingFunction[1]->GetBasis(jDegree, ParamCoord[1])
          * BlendingFunction[2]->GetBasis(kDegree, ParamCoord[2]);
        }

  return cart_coord;
}


su2double *CFreeFormDefBox::GetFFDGradient(su2double *val_coord, su2double *xyz) {

  unsigned short iDim, jDim, lmn[3];

  /*--- Set the Degree of the spline ---*/

  lmn[0] = lDegree; lmn[1] = mDegree; lmn[2] = nDegree;

  for (iDim = 0; iDim < nDim; iDim++) Gradient[iDim] = 0.0;

  for (iDim = 0; iDim < nDim; iDim++)
    for (jDim = 0; jDim < nDim; jDim++)
      Gradient[jDim] += GetDerivative2(val_coord, iDim, xyz,  lmn) *
      GetDerivative3(val_coord, iDim, jDim, lmn);

  return Gradient;

}

void CFreeFormDefBox::GetFFDHessian(su2double *uvw, su2double *xyz, su2double **val_Hessian) {

  unsigned short iDim, jDim, lmn[3];

  /*--- Set the Degree of the spline ---*/

  lmn[0] = lDegree; lmn[1] = mDegree; lmn[2] = nDegree;

  for (iDim = 0; iDim < nDim; iDim++)
    for (jDim = 0; jDim < nDim; jDim++)
      val_Hessian[iDim][jDim] = 0.0;

  /*--- Note that being all the functions linear combinations of polynomials, they are C^\infty,
   and the Hessian will be symmetric; no need to compute the under-diagonal part, for example ---*/

  for (iDim = 0; iDim < nDim; iDim++) {
    val_Hessian[0][0] += 2.0 * GetDerivative3(uvw, iDim,0, lmn) * GetDerivative3(uvw, iDim,0, lmn) +
    GetDerivative2(uvw, iDim,xyz, lmn) * GetDerivative5(uvw, iDim,0,0, lmn);

    val_Hessian[1][1] += 2.0 * GetDerivative3(uvw, iDim,1, lmn) * GetDerivative3(uvw, iDim,1, lmn) +
    GetDerivative2(uvw, iDim,xyz, lmn) * GetDerivative5(uvw, iDim,1,1, lmn);

    val_Hessian[2][2] += 2.0 * GetDerivative3(uvw, iDim,2, lmn) * GetDerivative3(uvw, iDim,2, lmn) +
    GetDerivative2(uvw, iDim,xyz, lmn) * GetDerivative5(uvw, iDim,2,2, lmn);

    val_Hessian[0][1] += 2.0 * GetDerivative3(uvw, iDim,0, lmn) * GetDerivative3(uvw, iDim,1, lmn) +
    GetDerivative2(uvw, iDim,xyz, lmn) * GetDerivative5(uvw, iDim,0,1, lmn);

    val_Hessian[0][2] += 2.0 * GetDerivative3(uvw, iDim,0, lmn) * GetDerivative3(uvw, iDim,2, lmn) +
    GetDerivative2(uvw, iDim,xyz, lmn) * GetDerivative5(uvw, iDim,0,2, lmn);

    val_Hessian[1][2] += 2.0 * GetDerivative3(uvw, iDim,1, lmn) * GetDerivative3(uvw, iDim,2, lmn) +
    GetDerivative2(uvw, iDim,xyz, lmn) * GetDerivative5(uvw, iDim,1,2, lmn);
  }

  val_Hessian[1][0] = val_Hessian[0][1];
  val_Hessian[2][0] = val_Hessian[0][2];
  val_Hessian[2][1] = val_Hessian[1][2];

}

su2double *CFreeFormDefBox::GetParametricCoord_Iterative(unsigned long iPoint, su2double *xyz, const su2double *ParamCoordGuess, CConfig *config) {

  su2double *IndepTerm, SOR_Factor = 1.0, MinNormError, NormError, Determinant, AdjHessian[3][3], Temp[3] = {0.0,0.0,0.0};
  unsigned short iDim, jDim, RandonCounter;
  unsigned long iter;

  su2double tol = config->GetFFD_Tol()*1E-3;
  unsigned short it_max = config->GetnFFD_Iter();
  unsigned short Random_Trials = 500;

  /*--- Allocate the Hessian ---*/

  Hessian = new su2double* [nDim];
  IndepTerm = new su2double [nDim];
  for (iDim = 0; iDim < nDim; iDim++) {
    Hessian[iDim] = new su2double[nDim];
    ParamCoord[iDim] = ParamCoordGuess[iDim];
    IndepTerm [iDim] = 0.0;
  }

  RandonCounter = 0; MinNormError = 1E6;

  /*--- External iteration ---*/

  for (iter = 0; iter < (unsigned long)it_max*Random_Trials; iter++) {

    /*--- The independent term of the solution of our system is -Gradient(sol_old) ---*/

    Gradient = GetFFDGradient(ParamCoord, xyz);

    for (iDim = 0; iDim < nDim; iDim++) IndepTerm[iDim] = - Gradient[iDim];

    /*--- Hessian = The Matrix of our system, getHessian(sol_old,xyz,...) ---*/

    GetFFDHessian(ParamCoord, xyz, Hessian);

    /*--- Adjoint to Hessian ---*/

    AdjHessian[0][0] = Hessian[1][1]*Hessian[2][2]-Hessian[1][2]*Hessian[2][1];
    AdjHessian[0][1] = Hessian[0][2]*Hessian[2][1]-Hessian[0][1]*Hessian[2][2];
    AdjHessian[0][2] = Hessian[0][1]*Hessian[1][2]-Hessian[0][2]*Hessian[1][1];
    AdjHessian[1][0] = Hessian[1][2]*Hessian[2][0]-Hessian[1][0]*Hessian[2][2];
    AdjHessian[1][1] = Hessian[0][0]*Hessian[2][2]-Hessian[0][2]*Hessian[2][0];
    AdjHessian[1][2] = Hessian[0][2]*Hessian[1][0]-Hessian[0][0]*Hessian[1][2];
    AdjHessian[2][0] = Hessian[1][0]*Hessian[2][1]-Hessian[1][1]*Hessian[2][0];
    AdjHessian[2][1] = Hessian[0][1]*Hessian[2][0]-Hessian[0][0]*Hessian[2][1];
    AdjHessian[2][2] = Hessian[0][0]*Hessian[1][1]-Hessian[0][1]*Hessian[1][0];

    /*--- Determinant of Hessian ---*/

    Determinant = Hessian[0][0]*AdjHessian[0][0]+Hessian[0][1]*AdjHessian[1][0]+Hessian[0][2]*AdjHessian[2][0];

    /*--- Hessian inverse ---*/

    if (Determinant != 0) {
      for (iDim = 0; iDim < nDim; iDim++) {
        Temp[iDim] = 0.0;
        for (jDim = 0; jDim < nDim; jDim++) {
          Temp[iDim] += AdjHessian[iDim][jDim]*IndepTerm[jDim]/Determinant;
        }
      }
      for (iDim = 0; iDim < nDim; iDim++) {
        IndepTerm[iDim] = Temp[iDim];
      }
    }

    /*--- Update with Successive over-relaxation ---*/

    for (iDim = 0; iDim < nDim; iDim++) {
      ParamCoord[iDim] = (1.0-SOR_Factor)*ParamCoord[iDim] + SOR_Factor*(ParamCoord[iDim] + IndepTerm[iDim]);
    }

    /*--- If the gradient is small, we have converged ---*/

    if ((fabs(IndepTerm[0]) < tol) && (fabs(IndepTerm[1]) < tol) && (fabs(IndepTerm[2]) < tol))  break;

    /*--- Compute the norm of the error ---*/

    NormError = 0.0;
    for (iDim = 0; iDim < nDim; iDim++)
      NormError += IndepTerm[iDim]*IndepTerm[iDim];
    NormError = sqrt(NormError);

    MinNormError = min(NormError, MinNormError);

    /*--- If we have no convergence with Random_Trials iterations probably we are in a local minima. ---*/

    if (((iter % it_max) == 0) && (iter != 0)) {

      RandonCounter++;
      if (RandonCounter == Random_Trials) {
        cout << endl << "Unknown point: "<< iPoint <<" (" << xyz[0] <<", "<< xyz[1] <<", "<< xyz[2] <<"). Min Error: "<< MinNormError <<". Iter: "<< iter <<"."<< endl;
      }
      else {
        SOR_Factor = 0.1;
        for (iDim = 0; iDim < nDim; iDim++)
          ParamCoord[iDim] = su2double(rand())/su2double(RAND_MAX);
      }

    }

    /* --- Splines are not defined outside of [0,1]. So if the parametric coords are outside of
     *  [0,1] the step was too big and we have to use a smaller relaxation factor. ---*/

    if ((config->GetFFD_Blending() == BSPLINE_UNIFORM)     &&
        (((ParamCoord[0] < 0.0) || (ParamCoord[0] > 1.0))  ||
         ((ParamCoord[1] < 0.0) || (ParamCoord[1] > 1.0))  ||
         ((ParamCoord[2] < 0.0) || (ParamCoord[2] > 1.0)))) {

      for (iDim = 0; iDim < nDim; iDim++){
        ParamCoord[iDim] = ParamCoordGuess[iDim];
      }
      SOR_Factor = 0.9*SOR_Factor;
    }

  }

  for (iDim = 0; iDim < nDim; iDim++)
    delete [] Hessian[iDim];
  delete [] Hessian;
  delete [] IndepTerm;

  /*--- The code has hit the max number of iterations ---*/

  if (iter == (unsigned long)it_max*Random_Trials) {
    cout << "Unknown point: (" << xyz[0] <<", "<< xyz[1] <<", "<< xyz[2] <<"). Increase the value of FFD_ITERATIONS." << endl;
  }

  /*--- Real Solution is now ParamCoord; Return it ---*/

  return ParamCoord;

}

bool CFreeFormDefBox::GetPointFFD(CGeometry *geometry, CConfig *config, unsigned long iPoint) const {
  su2double Coord[3] = {0.0, 0.0, 0.0};
  unsigned short iVar, jVar, iDim;
  su2double X_0, Y_0, Z_0, Xbar, Ybar, Zbar;

  bool Inside = false;
  bool cylindrical = (config->GetFFD_CoordSystem() == CYLINDRICAL);
  bool spherical = (config->GetFFD_CoordSystem() == SPHERICAL);
  bool polar = (config->GetFFD_CoordSystem() == POLAR);

  unsigned short Index[5][7] = {
    {0, 1, 2, 5, 0, 1, 2},
    {0, 2, 7, 5, 0, 2, 7},
    {0, 2, 3, 7, 0, 2, 3},
    {0, 5, 7, 4, 0, 5, 7},
    {2, 7, 5, 6, 2, 7, 5}};
  unsigned short nDim = geometry->GetnDim();

  for (iDim = 0; iDim < nDim; iDim++)
    Coord[iDim] = geometry->nodes->GetCoord(iPoint, iDim);

  if (cylindrical) {

    X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

    Xbar =  Coord[0] - X_0; Ybar =  Coord[1] - Y_0; Zbar =  Coord[2] - Z_0;

    Coord[0] = sqrt(Ybar*Ybar + Zbar*Zbar);
    Coord[1] = atan2(Zbar, Ybar); if (Coord[1] > PI_NUMBER/2.0) Coord[1] -= 2.0*PI_NUMBER;
    Coord[2] = Xbar;

  }

  else if (spherical || polar) {

    X_0 = config->GetFFD_Axis(0); Y_0 = config->GetFFD_Axis(1);  Z_0 = config->GetFFD_Axis(2);

    Xbar =  Coord[0] - X_0; Ybar =  Coord[1] - Y_0; Zbar =  Coord[2] - Z_0;

    Coord[0] = sqrt(Xbar*Xbar + Ybar*Ybar + Zbar*Zbar);
    Coord[1] = atan2(Zbar, Ybar);  if (Coord[1] > PI_NUMBER/2.0) Coord[1] -= 2.0*PI_NUMBER;
    Coord[2] = acos(Xbar/Coord[0]);

  }

  /*--- 1st tetrahedron {V0, V1, V2, V5}
   2nd tetrahedron {V0, V2, V7, V5}
   3th tetrahedron {V0, V2, V3, V7}
   4th tetrahedron {V0, V5, V7, V4}
   5th tetrahedron {V2, V7, V5, V6} ---*/

  for (iVar = 0; iVar < 5; iVar++) {
    Inside = true;
    for (jVar = 0; jVar < 4; jVar++) {
      su2double Distance_Point = geometry->Point2Plane_Distance(Coord,
                                                                Coord_Corner_Points[Index[iVar][jVar+1]],
                                                                Coord_Corner_Points[Index[iVar][jVar+2]],
                                                                Coord_Corner_Points[Index[iVar][jVar+3]]);

      su2double Distance_Vertex = geometry->Point2Plane_Distance(Coord_Corner_Points[Index[iVar][jVar]],
                                                                 Coord_Corner_Points[Index[iVar][jVar+1]],
                                                                 Coord_Corner_Points[Index[iVar][jVar+2]],
                                                                 Coord_Corner_Points[Index[iVar][jVar+3]]);
      if (Distance_Point*Distance_Vertex < 0.0) Inside = false;
    }
    if (Inside) break;
  }

  return Inside;

}

void CFreeFormDefBox::SetDeformationZone(CGeometry *geometry, CConfig *config, unsigned short iFFDBox) const {
  su2double *Coord;
  unsigned short iMarker, iVar, jVar;
  unsigned long iVertex, iPoint;
  bool Inside = false;

  unsigned short Index[5][7] = {
    {0, 1, 2, 5, 0, 1, 2},
    {0, 2, 7, 5, 0, 2, 7},
    {0, 2, 3, 7, 0, 2, 3},
    {0, 5, 7, 4, 0, 5, 7},
    {2, 7, 5, 6, 2, 7, 5}};

  for (iMarker = 0; iMarker < config->GetnMarker_All(); iMarker++) {
    if (config->GetMarker_All_DV(iMarker) == YES) {
      for (iVertex = 0; iVertex < geometry->nVertex[iMarker]; iVertex++) {
        iPoint = geometry->vertex[iMarker][iVertex]->GetNode();

        Coord = geometry->nodes->GetCoord(iPoint);

        /*--- 1st tetrahedron {V0, V1, V2, V5}
         2nd tetrahedron {V0, V2, V7, V5}
         3th tetrahedron {V0, V2, V3, V7}
         4th tetrahedron {V0, V5, V7, V4}
         5th tetrahedron {V2, V7, V5, V6} ---*/

        for (iVar = 0; iVar < 5; iVar++) {
          Inside = true;
          for (jVar = 0; jVar < 4; jVar++) {
            su2double Distance_Point = geometry->Point2Plane_Distance(Coord,
                                                                   Coord_Corner_Points[Index[iVar][jVar+1]],
                                                                   Coord_Corner_Points[Index[iVar][jVar+2]],
                                                                   Coord_Corner_Points[Index[iVar][jVar+3]]);
            su2double Distance_Vertex = geometry->Point2Plane_Distance(Coord_Corner_Points[Index[iVar][jVar]],
                                                                    Coord_Corner_Points[Index[iVar][jVar+1]],
                                                                    Coord_Corner_Points[Index[iVar][jVar+2]],
                                                                    Coord_Corner_Points[Index[iVar][jVar+3]]);
            if (Distance_Point*Distance_Vertex < 0.0) Inside = false;
          }
          if (Inside) break;
        }
      }
    }
  }
}

su2double CFreeFormDefBox::GetDerivative1(su2double *uvw, unsigned short val_diff, unsigned short *ijk, unsigned short *lmn) const {

  unsigned short iDim;
  su2double value = 0.0;

  value = BlendingFunction[val_diff]->GetDerivative(ijk[val_diff], uvw[val_diff], 1);
  for (iDim = 0; iDim < nDim; iDim++)
    if (iDim != val_diff)
      value *= BlendingFunction[iDim]->GetBasis(ijk[iDim], uvw[iDim]);

  return value;

}

su2double CFreeFormDefBox::GetDerivative2 (su2double *uvw, unsigned short dim, const su2double *xyz, const unsigned short *lmn) const {

  unsigned short iDegree, jDegree, kDegree;
  su2double value = 0.0;

  for (iDegree = 0; iDegree <= lmn[0]; iDegree++)
    for (jDegree = 0; jDegree <= lmn[1]; jDegree++)
      for (kDegree = 0; kDegree <= lmn[2]; kDegree++) {
        value += Coord_Control_Points[iDegree][jDegree][kDegree][dim]
        * BlendingFunction[0]->GetBasis(iDegree, uvw[0])
        * BlendingFunction[1]->GetBasis(jDegree, uvw[1])
        * BlendingFunction[2]->GetBasis(kDegree, uvw[2]);
      }

  return 2.0*(value - xyz[dim]);
}

su2double CFreeFormDefBox::GetDerivative3(su2double *uvw, unsigned short dim, unsigned short diff_this, unsigned short *lmn) {

  unsigned short iDegree, jDegree, kDegree, iDim;
  su2double value = 0;

  unsigned short *ijk = new unsigned short[nDim];

  for (iDim = 0; iDim < nDim; iDim++) ijk[iDim] = 0;

  for (iDegree = 0; iDegree <= lmn[0]; iDegree++)
    for (jDegree = 0; jDegree <= lmn[1]; jDegree++)
      for (kDegree = 0; kDegree <= lmn[2]; kDegree++) {
        ijk[0] = iDegree; ijk[1] = jDegree; ijk[2] = kDegree;
        value += Coord_Control_Points[iDegree][jDegree][kDegree][dim] *
        GetDerivative1(uvw, diff_this, ijk, lmn);
      }

  delete [] ijk;

  return value;
}

su2double CFreeFormDefBox::GetDerivative4(su2double *uvw, unsigned short val_diff, unsigned short val_diff2,
                                       unsigned short *ijk, unsigned short *lmn) const {
  unsigned short iDim;
  su2double value = 0.0;

  if (val_diff == val_diff2) {
    value = BlendingFunction[val_diff]->GetDerivative(ijk[val_diff], uvw[val_diff], 2);
    for (iDim = 0; iDim < nDim; iDim++)
      if (iDim != val_diff)
        value *= BlendingFunction[iDim]->GetBasis(ijk[iDim], uvw[iDim]);
  }
  else {
    value = BlendingFunction[val_diff]->GetDerivative(ijk[val_diff],  uvw[val_diff],1) *
    BlendingFunction[val_diff2]->GetDerivative(ijk[val_diff2], uvw[val_diff2], 1);
    for (iDim = 0; iDim < nDim; iDim++)
      if ((iDim != val_diff) && (iDim != val_diff2))
        value *= BlendingFunction[iDim]->GetBasis(ijk[iDim], uvw[iDim]);
  }

  return value;
}

su2double CFreeFormDefBox::GetDerivative5(su2double *uvw, unsigned short dim, unsigned short diff_this, unsigned short diff_this_also,
                                      unsigned short *lmn) {

  unsigned short iDegree, jDegree, kDegree, iDim;
  su2double value = 0.0;

  unsigned short *ijk = new unsigned short[nDim];

  for (iDim = 0; iDim < nDim; iDim++) ijk[iDim] = 0;

  for (iDegree = 0; iDegree <= lmn[0]; iDegree++)
    for (jDegree = 0; jDegree <= lmn[1]; jDegree++)
      for (kDegree = 0; kDegree <= lmn[2]; kDegree++) {
        ijk[0] = iDegree; ijk[1] = jDegree; ijk[2] = kDegree;
        value += Coord_Control_Points[iDegree][jDegree][kDegree][dim] *
        GetDerivative4(uvw, diff_this, diff_this_also, ijk, lmn);
      }

  delete [] ijk;

  return value;
}
